#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import serial
import time
import logging
import struct

# 配置日志
logger = logging.getLogger(__name__)

# 通信协议参数
CMD_HEADER1 = 0xAA  # 指令头第一字节
CMD_HEADER2 = 0xBB  # 指令头第二字节
CMD_FOOTER = 0x55    # 指令尾

class SerialHandler:
    def __init__(self, serial_port='/dev/ttyUSB0', baud_rate=38400):
        """
        初始化串口处理器
        :param serial_port: 串口设备名称
        :param baud_rate: 波特率
        """
        self.serial_port = serial_port
        self.baud_rate = baud_rate
        self.serial_conn = None
        self.running = False
        
    def connect(self):
        """
        连接串口设备
        """
        try:
            self.serial_conn = serial.Serial(
                port=self.serial_port,
                baudrate=self.baud_rate,
                timeout=1
            )
            logger.info(f"成功连接到串口 {self.serial_port} (波特率: {self.baud_rate})")
            return True
        except serial.SerialException as e:
            logger.error(f"无法连接到串口: {e}")
            return False
    
    def is_connected(self):
        """
        检查串口是否已连接
        """
        return self.serial_conn is not None and self.serial_conn.is_open
    
    def send_command(self, vx, vy, vw):
        """
        发送控制命令到串口
        :param vx: X方向速度 (m/s)
        :param vy: Y方向速度 (m/s)
        :param vw: 角速度 (rad/s)
        :return: 是否成功发送
        """
        if not self.is_connected():
            logger.error("串口未连接，无法发送命令")
            return False
        
        try:
            # 单位转换：
            # vx, vy: m/s -> cm/s (乘以100)
            # vw: rad/s -> 0.1rad/s (乘以10)
            vx_cm = int(vx * 100)  # 转换为cm/s并取整
            vy_cm = int(vy * 100)  # 转换为cm/s并取整
            vw_01rad = int(vw * 10)  # 转换为0.1rad/s并取整
            
            # 限制数值范围
            vx_cm = max(-32768, min(32767, vx_cm))  # 16位有符号整数范围
            vy_cm = max(-32768, min(32767, vy_cm))  # 16位有符号整数范围
            vw_01rad = max(-128, min(127, vw_01rad))  # 8位有符号整数范围
            
            # 构造二进制数据
            # 指令格式: [头部(0xAA 0xBB)][Vx高][Vx低][Vy高][Vy低][Wz][校验和][尾(0x55)]
            vx_high = (vx_cm >> 8) & 0xFF  # 高字节
            vx_low = vx_cm & 0xFF  # 低字节
            vy_high = (vy_cm >> 8) & 0xFF  # 高字节
            vy_low = vy_cm & 0xFF  # 低字节
            
            # 计算校验和 (异或所有数据字节)
            checksum = CMD_HEADER1 ^ CMD_HEADER2 ^ vx_high ^ vx_low ^ vy_high ^ vy_low ^ (vw_01rad & 0xFF)
            
            # 构造完整指令
            command = bytearray([CMD_HEADER1, CMD_HEADER2, vx_high, vx_low, vy_high, vy_low, vw_01rad & 0xFF, checksum, CMD_FOOTER])
            
            # 发送指令前清空接收缓冲区
            self.serial_conn.reset_input_buffer()
            
            # 发送指令
            self.serial_conn.write(command)
            self.serial_conn.flush()  # 确保所有数据都被发送出去
            
            # 添加发送延时，确保Arduino有足够时间处理
            time.sleep(0.05)  # 50ms延时
            
            # 输出日志，以十六进制格式显示
            hex_cmd = ' '.join([f'0x{b:02X}' for b in command])
            logger.info(f"已发送到串口: {hex_cmd}")
            logger.info(f"  原始值: vx={vx}m/s, vy={vy}m/s, vw={vw}rad/s")
            logger.info(f"  转换值: vx={vx_cm}cm/s, vy={vy_cm}cm/s, vw={vw_01rad}*0.1rad/s")
            
            return True
        except Exception as e:
            logger.error(f"发送命令到串口失败: {e}")
            return False
    
    def read_data(self):
        """
        从串口读取数据
        :return: 读取到的数据或None
        """
        if not self.is_connected():
            return None
        
        try:
            if self.serial_conn.in_waiting > 0:
                data = self.serial_conn.readline().decode('utf-8').strip()
                logger.debug(f"从串口读取: {data}")
                return data
        except Exception as e:
            logger.error(f"从串口读取数据失败: {e}")
        
        return None
    
    def close(self):
        """
        关闭串口连接
        """
        if self.serial_conn and self.serial_conn.is_open:
            try:
                self.serial_conn.close()
                logger.info("串口连接已关闭")
            except Exception as e:
                logger.error(f"关闭串口连接时出错: {e}")
            finally:
                self.serial_conn = None
                
    def calculate_checksum(self, data):
        """
        计算校验和
        :param data: 要计算校验和的数据字节列表
        :return: 校验和字节
        """
        checksum = 0
        for byte in data:
            checksum ^= byte
        return checksum
